/* Includes ----------------------------------------------------------------- */
#include "filter.h"
/* Private define ----------------------------------------------------------- */
#include <string.h>
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

/*
 * @brief   一维卡尔曼滤波器的具体实现。实现过程完全与硬件无关
 *          MPU6050的卡尔曼滤波器参考参数 Q：10 R：400 *
 */

/**
 * @brief 创建一个卡尔曼滤波器
 *
 * @param p 滤波器
 * @param T_Q 系统噪声协方差
 * @param T_R 测量噪声协方差
 */
int8_t Filter_KalmanInit(Filter_Kalman_t* p, float T_Q) {
  if (p == NULL) return -1;
  p->Q_angle = 0.001;
  p->Q_gyro = 0.005;
  p->R_angle = 0.5;
  p->H = 1.f;
  p->dt = T_Q;

  p->P[0][0] = 1;
  p->P[0][1] = 0;
  p->P[1][0] = 0;
  p->P[1][1] = 1;

  return 0;
}

/**
 * @name   KalmanFilter
 * @brief  卡尔曼滤波器
 * @param  p:  滤波器
 * @param  dat:待滤波数据
 * @retval 滤波后的数据
 */

float Filter_KalmanApply(float acc, float gyro, Filter_Kalman_t* p) {
  /*  先验估计 */
  float dt = p->dt;
  float C_0 = p->H;
  float P[2][2];
  P[0][0] = p->P[0][0];
  P[0][1] = p->P[0][1];
  P[1][0] = p->P[1][0];
  P[1][1] = p->P[1][1];
  p->out += (gyro - p->Q_bias) * dt;

  /* 计算误差协方差 */
  P[0][0] += p->Q_angle - (P[0][1] + P[1][0]) * dt + P[1][1] * dt * dt;
  P[0][1] += -P[1][1] * dt;
  P[1][0] += -P[1][1] * dt;
  P[1][1] += p->Q_gyro;

  /* 计算卡尔曼增益 */
  float PCt_0 = C_0 * P[0][0];
  float PCt_1 = C_0 * P[1][0];

  /* 修正估计 */
  float E = p->R_angle + C_0 * PCt_0;

  float K_0 = PCt_0 / E;
  float K_1 = PCt_1 / E;

  float angel_err = acc - p->out;
  p->out += K_0 * angel_err;    /* 最优角度 */
  p->Q_gyro = gyro - p->Q_bias; /* 最优角速度 */
  p->Q_bias += K_1 * angel_err; /* 最优零漂 */

  /* 更新误差协方差 */
  float t_0 = PCt_0;
  float t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;

  p->P[0][0] = P[0][0];
  p->P[0][1] = P[0][1];
  p->P[1][0] = P[1][0];
  p->P[1][1] = P[1][1];
  return p->out;
}
int8_t Filter_LowPassInit(Filter_LowPass_t* filter, float k) {
  if (filter == NULL) return -1;
  filter->k = k;
  return 0;
}

float Filter_LowPassApply(Filter_LowPass_t* filter, float new) {
  if (filter == NULL) return -1;
  filter->out = filter->k * new + (1 - filter->k) * filter->out;
  return filter->out;
}

int8_t Filter_ComplyInit(ComplyFilter_t* filter, float k, uint32_t freq) {
  if (filter == NULL) return -1;
  filter->k = k;
  filter->freq = freq;
  return 0;
}

float Filte_ComplyApply(ComplyFilter_t* filter, float angle, float gyro) {
  if (filter == NULL) return -1;
  filter->out =
      filter->k * angle + (1 - filter->k) * (gyro / filter->freq + filter->out);
  return filter->out;
}
